#include "cameraReconstruction.h"

ListBase xLines={0,0};
ListBase yLines={0,0};
ListBase zLines={0,0};

int ActualAxis=X;

void addLine(int axis, vec2i initPoint, vec2i endPoint)
{
	
	//Add line to structure
	MyLine *line = (MyLine*)malloc(sizeof(MyLine));
	line->init=initPoint;
	line->end=endPoint;
	 /* BLI_addtail will add item at the end of DynamicList */ 
	if(axis==X)
		BLI_addtail(&xLines, line);
	else if(axis==Y)
		BLI_addtail(&yLines, line);
	else if(axis==Z)
		BLI_addtail(&zLines, line);
}

void calculateLineIntersection(MyLine *l1, MyLine *l2, vec2f *res)
{
	float a, a1, b, b1;

	a= (float)( l1->end.y - l1->init.y  )/(float)( l1->end.x - l1->init.x );
	a1=(float)( l2->end.y - l2->init.y  )/(float)( l2->end.x - l2->init.x );
	
	b= (float)l1->init.y - a*(float)(l1->init.x);
	b1=(float)l2->init.y - a1*(float)(l2->init.x);

	res->x= ( b1-b )/( a-a1 );
	res->y= a*res->x + b;
	
	printf("%f\t%f\t%d\n",res->x, res->y,ActualAxis);	
}

int sortByY(LinkData* d1, LinkData* d2)
{
	vec2f *p1=(vec2f*)(d1->data);
	vec2f *p2=(vec2f*)(d2->data);
	if(p1->y < p2->y)
		return 0;
	else
		return 1;
}

int sortByX(LinkData* d1, LinkData* d2)
{
	vec2f *p1=(vec2f*)(d1->data);
	vec2f *p2=(vec2f*)(d2->data);
	if(p1->x < p2->x)
		return 0;
	else
		return 1;
}

vec2f calculateOptimalIntersection(ListBase *points)
{
	// Basic Statistic: N/2 data or can calculate average of c1 to c3 
	// Where N is the number of points and C1= N/4 and c3= 3N/4
	// This can be an aproximation of point
	// Review paper for search the optimal solutuion
	
	// Now we go to use a (p_x(N/2) + p_x((N/2)+1)) if N%2==0
	// else p_x((N+1)/2)
	// and same with p_y
	// With sorted p_x and p_y
	vec2f p;
	LinkData *aux;
	LinkData *aux1;

	int N=BLI_countlist(points);
	if(N>0){
	//Find average X point
	BLI_sortlist(points, &sortByX);
	if(N%2){
		aux= BLI_findlink(points, (N+1)/2 );
		p.x=((vec2f*)(aux->data))->x;
	}else{
		aux= BLI_findlink(points, N/2 );
		aux1= BLI_findlink(points, (N/2)+1 );
		p.x=(((vec2f*)(aux->data))->x+((vec2f*)(aux1->data))->x)/2.0f;
	}

	//Find average X point
	BLI_sortlist(points, &sortByY);
	if(N%2){
		aux= BLI_findlink(points, (N+1)/2 );
		p.y=((vec2f*)(aux->data))->y;
	}else{
		aux= BLI_findlink(points, N/2 );
		aux1= BLI_findlink(points, (N/2)+1 );
		p.y=(((vec2f*)(aux->data))->y+((vec2f*)(aux1->data))->y)/2.0f;
	}
	printf("Average point from axis %d : %f\t%f\n",ActualAxis,p.x, p.y);
	return p;
	}
	return;
}

void calculateIntersection(ListBase* lines,vec2f* point)
{
	ListBase aux_points={0,0};
	Link *item=(Link*)lines->first;	
	while(item)
	{
		Link *item1=(Link*)item->next;
		while(item1)
		{
			vec2f *p=(vec2f*)malloc(sizeof(vec2f));
			calculateLineIntersection((MyLine*)item,(MyLine*)item1, p);			
			LinkData *ld=(LinkData*)malloc(sizeof(LinkData));
			ld->data=p;
			BLI_addtail(&aux_points, ld);
			item1=item1->next;	
		}
		item=item->next;
	}
	vec2f res=calculateOptimalIntersection(&aux_points);
	point->x=res.x;
	point->y=res.y;
}
void printfLines(){
	Link* item=xLines.first;
	printf("\n\n#Xlines\n");
	while(item){
		MyLine *l=(MyLine*)item;
		printf("%d\t%d\t%d\t%d\n",l->init.x, l->init.y, l->end.x-l->init.x, l->end.y-l->init.y);
		item=item->next;
	}
	item=yLines.first;
	printf("\n\n#Ylines\n");
	while(item){
		MyLine *l=(MyLine*)item;
		printf("%d\t%d\t%d\t%d\n",l->init.x, l->init.y, l->end.x-l->init.x, l->end.y-l->init.y);
		item=item->next;
	}
	item=zLines.first;
	printf("\n\n#Zlines\n");
	while(item){
		MyLine *l=(MyLine*)item;
		printf("%d\t%d\t%d\t%d\n",l->init.x, l->init.y, l->end.x-l->init.x, l->end.y-l->init.y);
		item=item->next;
	}
}

void calculatePrincipalPoint()
{
	float m,b,m1,b1;
	//Recta perpendicular a X Y que pasa por Z
	m=(xpoint.x-ypoint.x)/(xpoint.y-ypoint.y);
	b=zpoint.y-m*zpoint.x;
	//Recta perpendicular a X Z y que pasa por Y
	m1=(xpoint.x-zpoint.x)/(xpoint.y-zpoint.y);
	b1=ypoint.y-m1*ypoint.x;
	
	principalpoint.x=(b1-b)/(m-m1);
	principalpoint.y=m*principalpoint.x+b;
	printf("Principal point: %f\t%f\n", principalpoint.x, principalpoint.y);
}
void calculateFocalLength()
{
	float dist_v3_pp, dist_pp_px;
	vec2f px;
	float m=(xpoint.y-ypoint.y)/(xpoint.x-ypoint.x);
	float m1=1/m;
	float b=xpoint.y-m*xpoint.x;
	float b1=zpoint.y-m1*zpoint.x;
	px.x=(b1-b)/(m-m1);
	px.y=m*px.x + b;

	dist_v3_pp=sqrt( pow(principalpoint.x-zpoint.x,2) + pow(principalpoint.y-zpoint.y,2) );
	dist_pp_px=sqrt( pow(px.x-principalpoint.x,2) + pow(px.y-principalpoint.y,2)  );

	focalLength=sqrt(dist_v3_pp*dist_pp_px);

	printf("Focal Length: %f\n", focalLength );
}

void calculateCameraParams()
{
	ActualAxis=X;
	calculateIntersection(&xLines,&xpoint);
	//printf("\n\n");
	ActualAxis=Y;
	calculateIntersection(&yLines,&ypoint);
	//printf("\n\n");
	ActualAxis=Z;
	calculateIntersection(&zLines,&zpoint);
	//printf("\n\n");
	//printfLines();

	//We now hve the 2 or 3 vanishing points.
	calculatePrincipalPoint();
	//we now go to get the
	calculateFocalLength(); 
}


